#ifndef RSIM_SYSTEMDATA_H
#define RSIM_SYSTEMDATA_H

/* -------------------------------------------------------------------------- *
 * File: SystemData.h                                                         *
 * Authors: Kishor Bhalerao                                                   *
 * Email : kishor8dm@gmail.com                                                *
 * Contributors:                                                              *
 * Email:                                                                     *
 * copyright (c) 2010 Authors.                                                *
 *                                                                            *
 * This program is free software: you can redistribute it and/or modify it    *
 * under the terms of the GNU General Public License as published by the Free *
 * Software Foundation, either version 3 of the License, or any later version.*
 *                                                                            *
 * This program is distributed in the hope that it will be useful, but        *
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY *
 * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License   *
 * for more details. (http://www.gnu.org/licenses/)                           *
 * -------------------------------------------------------------------------- */

#include "List.h"
#include "Index.h"
#include "SystemDataUtils.h"

namespace RSIM{
	
class MultibodySystem;
class State;
class Transform;
class Constraint;
class CoordinateProjection;

class SystemData:public SystemDataUtils{
        private:
                /** Once finalized or set to true, no more data can be added */
                bool isFinalized_;
                
                /** Number of systems stored in this class */
                int nSys_;
                
                /** This function searches for coupled algebraic constraints in the system. 
                Calling this function sets the variable ccouple_.*/
                // Not entirely happy with the current implementation. This is a preprocessing
                // step and should not affect the simulation speed anyway. This is currently 
                // is a very crude implementation and can be improved. 
                void initAlgebraicConstraints();                                
                
                /** Object used to perform coordinate projection */
                CoordinateProjection *cpr_;
                
        public:
                /** Constraints coupling information stored in ccouple_ */
                List < List<int> > ccouple_;
                
                /** Pointers to all the objects of the class MultibodySystem */		
                List <MultibodySystem*> system_;
                
                /** 
                Pointers to all the objects of the class State. 
                */
                List <State*> state_;

                /** List of all the constraints in the system*/
                List <Constraint*> constraints_;
                
                SystemData():SystemDataUtils(),isFinalized_(false),nSys_(0),cpr_(NULL){}
                
                ~SystemData();
                
                /**
                This calls the function MultibodySystem::updSystemPosition(const State& state)
                for each multibody system in SystemData. Calling this function *DOES NOT* ensure
                that the position level algebraic constraints, if defined in the system are satisfied.
                */
                void updSystemPosition();
                
                /** If algebraic constraints are defined in the system, this function attempts to satisfy
                them to a desired tolerance.*/
                void attemptSatisfyPositionLevelConstraints();
                
                /** Add pointers to System and corresponding State */
                void addSystemData(MultibodySystem *Sys, State *state);
                
                /** Finalize the system Data. No more data can be added after this function call. 
                This initializes various iterators and does the preprocessing for algebraic constraints (closed loops)
                if any are defined in main(). This function should be called just once. Once executed, any other function
                calls are ignored*/
                void finalize();
                
                /** 
                This can be used to set the parameters for coordinate projection.
                @param Tol This is the constraint tolerance and the default value is 1.0E-6 
                @param iter This is the number of iterations attempted during coordinate projection to satisfy constraints upto
                the specified tolerance. The default value is 50.
                */
                void setCoordinateProjectionParameters(const double& Tol, const int& niter);
                        
                /** 
                This function defines a kinematic constraint between two bodies in a multibody system. 
                Cannot be used to define a constraint between a body and ground. Use constraintToGround
                for that.
                */
                void constrain( const char *str, 				// Type of Kinematic Constraint
                                const Transform& Parent_T_PM, 			// Transform T_PM
                                const SystemIndex& ParentSysId, 		// Index of the parent MultibodySystem
                                const NodeIndex& ParentSysNodeId,		// Index of body to be contrained in parent Multibody system			       
                                const Transform& Child_T_BF,			// Transform T_BF
                                const SystemIndex& ChildSysId,                  // Index of the child MultibodySystem
                                const NodeIndex& ChildSysNodeId                 // Index of body to be constrained in child Multibody system
                        );
                        
                void constrainToGround( const char *str,                                // Type of Kinematic Constraint
                                        const Transform& Parent_T_PM,                   // Transform T_PM
                                        const SystemIndex& ParentSysId,                 // Index of the parent MultibodySystem
                                        const NodeIndex& ParentSysNodeId,               // Index of body to be constrained in parent Multibody system                           
                                        const Transform& Child_T_BF                     // Transform T_BF
                                        ) ;
                        
}; // class SystemData

} // namespace RSIM


#endif